%% execution = hybrid_step(q_i, robot)
%
%  Description:
%   The hybrid_step function takes in an initial state and handles to ODE,
%   reset and guard maps and returns a list of data points calculated by
%   ODE integration.
%
%  Operational Description:
%
%  Arguments:
%   q_i    - The initial condition of the continuous state variable.
%   robot  - A struct containing the model parameters of the system, the
%            guards, reset maps and constraints associated with the current
%            discrete state and the current discrete state of the system.
%
%  Return Values:
%   execution - a struct containing the results of the step.
%       next_state     - the discrete state into which the system should
%                         transition.
%       integral_curve - the results of the ODE integration.
%       reset_coords   - the new values for the continuous state variable
%                        given by the reset map.
%
function execution = hybrid_step(x_i, d_i, robot)
% t_max - maximum time allowed for this state to hit guard
%
t_max = 2;
execution.discrete_state = d_i;

%% Check if we start on the guard and reset immediately.
%
% eventnum = checkguard(x_i, p_i, robot);
eventnum = 0;
if eventnum ~= 0
    %% Apply the reset map to the initial conditions.
    %
    [rest_xi, rest_di] = robot.system.resets(x_i, d_i);
    
    %% Return the solution to this execution along with the interval over
    %  which execution.integral_curve = [0 q_i];
    execution.next_xi = rest_xi;
    execution.next_di = rest_di;
    
    execution.integral_curve = [];
    execution.time = [];
    execution.interval = [0 0];

    %% Execution of the following else block indicates that we did not 
    % start on a guard so we call the integrator.
    %
else
    %% Perform integration over the default time interval.  Use the
    %  provided ODE and pass it the initial conditions.
    %
    options = odeset( ...
        'Events', @(t, x) guard_map(t, x, d_i), ...
        'MaxStep', 1e-3);
    
    sol = ode45(robot.system.odefcn, ...
        [0 t_max], x_i, options);
    
    execution.time = sol.x;
    execution.interval = sol.x([1 end]);
        
    execution.integral_curve = sol.y;

    % Make sure an event occured.
    if ~isempty(sol.xe)
        [execution.next_xi, execution.next_di] = ...
            robot.system.resets(sol.ye(:, end), d_i);
        eventnum = sol.ie(end);
    else
        execution.next_xi = sol.y(:, end);
        execution.next_pi = p_i;
        eventnum = 1;
    end
    
end

% Return the number of the event that occured.
execution.event = eventnum;
% 
% % Now we find the next state and return it in our execution struct.
% try
%     execution.next_state = getnextstate(robot.current_state, ...
%         execution.event);
% catch ex
%     disp([ex.identifier '->' ex.message]);
%     rethrow(ex);
% end

%% checkguard() is used to check if the initial condition is on a guard
% and returns the guard number standardized to this domain, i.e. guard
% 1, 2, etc.
%
%     function eventnum = checkguard(x_c, p_c, robot)
%         % By default, we assume no event occurs.
%         eventnum = 0;
%         % if robot.system.guards(0, q_c) < 0
%         %     error('bad guard definition.  should guard be negated?');
%         % end
%         
%         % Check that h ~ 0 and that dhdq <= 0.
%         %
%         if p_c(1) == 1 || p_c(1) == 2
%             if abs(robot.system.guards(x_c)) <= 1e-6 && ...
%                     robot.system.dguards(x_c) <= 0
%                 eventnum = 1;
%                 disp(['Note: Instantaneous transition encountered ' ...
%                 'from domain ' num2str(p_c(1))]);
%             end
%         else
%             if robot.system.guards(x_c) <= 1e-7
%                 eventnum = 1;
%                 disp(['Note: Instantaneous transition encountered ' ...
%                 'from domain ' num2str(p_c(1))]);
%             end
%         end
%     end
end
